import numpy as np
import cv2
import time
from my_A_star_algorithm import *


def read_pipline(cap,read_time):
    t=time.perf_counter()
    while(time.perf_counter()-t<read_time):
        img1=cap.read()
def im2coordinate(img1):
    if img1.dtype=='uint8':
        pass
    else:
        img1=(img1*255).astype('uint8')

    img_line=img1[600:650,:]
    img_line_gray=cv2.cvtColor(img_line,cv2.COLOR_RGB2GRAY)
    img_line_gray= cv2.GaussianBlur(img_line_gray, (3, 3), 0)
    img_line_mean_keepdims = np.mean(img_line_gray, axis=0,keepdims=1)
    img_line_mean=img_line_mean_keepdims.squeeze()

    fuzzy_index=np.argmin(cv2.blur(img_line_mean_keepdims, (1, 10)))

    threshold=np.mean(img_line_mean[fuzzy_index-5:fuzzy_index+5])+15

    indexs=np.where(img_line_mean<threshold)[0]
    img_line_mean=img_line_mean*0+1
    img_line_mean[indexs]=0

    lens=[]
    mid_indexs=[]
    length=0
    for i in range(1280):
        if img_line_mean[i]==0 and length==0:
            i_s=i
            length=length+1
        elif img_line_mean[i]==0:
            length=length+1
        elif img_line_mean[i]==1 and length!=0:
            i_e=i
            mid_indexs.append((i_e+i_s)//2)
            lens.append(length)
            length=0
        else:
            pass
    if len(lens)==0:
        return 666
    else:
        lens_array=np.array(lens)
        x_record=mid_indexs[np.argmax(lens_array)]
    return(x_record)


def angle_cos(p0, p1, p2):
    d1, d2 = (p0-p1).astype('float'), (p2-p1).astype('float')
    return abs( np.dot(d1, d2) / np.sqrt( np.dot(d1, d1)*np.dot(d2, d2) ) )

def get_4_coordinate(img1_bin):#输入一个二值化的图片，获取四周4个角点的坐标
    contours, _hierarchy = cv2.findContours(img1_bin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    # for cnt in contours:
    #     cv2.drawContours(img1_filterred, [cnt], 0, (0, 0, 255), 5)
    # cv2.namedWindow("linshi",cv2.WINDOW_AUTOSIZE)
    # cv2.imshow("linshi",img1_filterred)
    # cv2.waitKey(5000)
    # cv2.destroyAllWindows()

    index = 0
    squares = []
    squares_sorted=[]
    for cnt in contours:
        cnt_len = cv2.arcLength(cnt, True)  # 计算轮廓周长
        if cnt_len>300 or cnt_len<20:
            continue
        cnt = cv2.approxPolyDP(cnt, 0.02 * cnt_len, True)  # 多边形逼近
        # 条件判断逼近边的数量是否为4，轮廓面积是否大于1000，检测轮廓是否为凸的
        if len(cnt) == 4 and cv2.contourArea(cnt) > 500 and cv2.isContourConvex(cnt):
            cnt = cnt.reshape(-1, 2)
            max_cos = np.max([angle_cos(cnt[i], cnt[(i + 1) % 4], cnt[(i + 2) % 4]) for i in range(4)])
            # 只检测矩形（cos90° = 0）
            if max_cos < 0.2:
                # 检测四边形（不限定角度范围）
                # if True:
                index = index + 1
                #cv2.putText(img1_filterred, ("#%d" % index), (cx, cy), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 255), 2)
                squares.append(cnt)

    return squares


def get_box(mask):#返回的x代表高度，y代表宽度，有问题的，记得反一下
    y = mask.sum(axis=1)
    x = mask.sum(axis=0)
    weight = 0
    count = 0
    length = 0
    for i in range(len(x)):
        if x[i] > 1:
            weight = x[i] * i + weight
            count = count + x[i]
            length = length + 1
    if count>2:
        x_wmid = round(weight / count)
        x_half_length = round(length / 2)
    else:
        x_wmid = 0
        x_half_length = 0
    weight = 0
    count = 0
    length = 0
    for i in range(len(y)):
        if y[i] > 1:
            weight = y[i] * i + weight
            count = count + y[i]
            length = length + 1
    if count > 2:
        y_wmid = round(weight / count)
        y_half_length = round(length / 2)
    else:
        y_wmid = 0
        y_half_length = 0

    return x_wmid,y_wmid,x_half_length,y_half_length


class my_timer():
    def __init__(self):
        self.ts = 0  # 自己的坐标

    def tic(self):
        self.ts = time.perf_counter()

    def toc(self):
        print("time used:"+str(time.perf_counter()-self.ts))



def crop_treasure_picture(img1):
    if img1[0][0][0].dtype!='uint8':
        img1=(img1*255).astype("uint8")
    fx=0.5
    fy=0.5
    img1=cv2.resize(img1,(0,0),fx=fx,fy=fy)
    img1 = cv2.medianBlur(img1, 3)
    img_transformed=np.abs(img1[:,:,0]-np.mean(img1,axis=2))+np.abs(img1[:,:,1]-np.mean(img1,axis=2))+np.abs(img1[:,:,2]-np.mean(img1,axis=2))
    img_transformed=img_transformed.astype("uint8")
    if np.max(img_transformed)<30:
        return None,[0,0,0,0]
    threshold_treasure=(np.max(img_transformed)-np.min(img_transformed))/2
    # hist = cv2.calcHist([img_transformed], [0], None, [180], [0, 179]) #绘制各个通道的直方图
    # plt.plot(hist)
    # plt.show()
    index1=np.where(img_transformed>threshold_treasure)
    mask=np.zeros_like(img1[:,:,0])
    mask[index1]=1
    kernel = np.ones((3,3),np.uint8)              #设置kenenel大小
    mask = cv2.dilate(mask,kernel,iterations=1)  # 腐蚀去除白噪点
    mask = cv2.erode(mask,kernel,iterations=1)  # 腐蚀去除白噪点

    num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(mask)
    if num_labels>8:
        return None, [0, 0, 0, 0]
    if num_labels==2:
        #print(1)
        index = np.where(labels == 1)
        #x_wmid,y_wmid,x_half_length,y_half_length=get_box(mask)
        crop_treasure = img1[np.min(index[0]):np.max(index[0]),
                        np.min(index[1]):np.max(index[1]), :]
        # cv2.rectangle(img1, (y_wmid - y_half_length, x_wmid - x_half_length),
        #               (y_wmid + y_half_length, x_wmid + x_half_length), (0, 255, 0), 2)
        # plt.imshow(crop_treasure)
        # plt.imsave("./neural_net_classify/1.jpg",crop_treasure)
        # plt.show()
        return crop_treasure,[int(np.min(index[0])//fy),int(np.max(index[0])//fy),int(np.min(index[1])//fx),int(np.max(index[1])//fx)]
    elif num_labels > 2:
        for i in range(1, num_labels):
            index = np.where(labels == i)
            if len(index[0]) > 200:
                mask_now = np.zeros_like(mask)
                mask_now[index] = 1
                x_wmid,y_wmid,x_half_length,y_half_length=get_box(mask_now)
                hw_rate = y_half_length / x_half_length
                if hw_rate > 1.5 and hw_rate < 3.5:
                    crop_treasure = img1[np.min(index[0]):np.max(index[0]),
                                    np.min(index[1]):np.max(index[1]), :]
                    return crop_treasure,[int(np.min(index[0])//fy),int(np.max(index[0])//fy),int(np.min(index[1])//fx),int(np.max(index[1])//fx)]
        return None,[0,0,0,0]
    else:
        return None, [0,0,0,0]
def get_line_mat_center(line_array):
    mat = 0
    num_count = 0
    for i in range(len(line_array)):
        mat = mat + i * line_array[i]
        num_count = num_count + line_array[i]
        mat_center = int(mat / (num_count+0.000000001))
    return mat_center


def track_line(img1,direction,mask_track):
    if img1[0][0][0].dtype != 'uint8':
        img1 = (img1 * 255).astype("uint8")
    img1 = img1[-300:, :, :]
    img1 = cv2.resize(img1, (0, 0), fx=0.3, fy=0.3)
    h,w=img1.shape[0],img1.shape[1]
    img1_gray = cv2.cvtColor(img1, cv2.COLOR_RGB2GRAY)
    #
    img1_filtered = cv2.medianBlur(img1_gray, 5)
    
    threshold = np.min(cv2.blur(img1_gray, (10, 10))) +35 
    ret, thresh1 = cv2.threshold(img1_gray, threshold, 255, cv2.THRESH_BINARY_INV)

    final_track_map = mask_track * thresh1
    num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(final_track_map)
    if centroids.shape[0]>1:
        centroids = centroids[1:, :]
        if direction==1:
            index_centroids = np.argmin(np.abs(centroids[:, 0]-w//2))
        elif direction==2:
            index_centroids = np.argmin(centroids[:, 0])
        elif direction == 3:
            index_centroids = np.argmax(centroids[:, 0])
        else:
            return None,None
        return(int(centroids[index_centroids, 0]),int(centroids[index_centroids, 1]))
    else:
        return None,None
